#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#xyz在10秒内交替发出（0, 5, 0）和（0, 0, 0）
#为适应真实机械臂改称（0, 2, 0）

import rospy
from geometry_msgs.msg import WrenchStamped

WRENCH_TOPIC = "/wrench_fake"
TOPIC_HZ = 125.0

def main():
    rospy.init_node("wrench_signal_generate")
    wrench_pub = rospy.Publisher(WRENCH_TOPIC, WrenchStamped, queue_size=5)
    rospy.sleep(5.0)
    rate = rospy.Rate(TOPIC_HZ)
    wrench_msg = WrenchStamped()
    t = 0

    while not rospy.is_shutdown():
        # Calculate the current cycle time (0 to 20 seconds)
        cycle_time = t % 20

        # First 10 seconds: force is (0, 5, 0)
        if cycle_time < 10:
            wrench_msg.wrench.force.x = 0
            wrench_msg.wrench.force.y = 2
            wrench_msg.wrench.force.z = 0
        # Next 10 seconds: force is (0, 0, 0)
        else:
            wrench_msg.wrench.force.x = 0
            wrench_msg.wrench.force.y = 0
            wrench_msg.wrench.force.z = 0

        # Publish the message
        wrench_pub.publish(wrench_msg)

        # Increment time
        t += 1 / TOPIC_HZ
        rate.sleep()

if __name__ == "__main__":
    main()
